#include "track.h"
/*---------------------------------------------------------
	SORT 目标追踪
----------------------------------------------------------*/
void track_process(int id)
{
	cpu_set_t mask;
	int cpuid = id;

	CPU_ZERO(&mask);
	CPU_SET(cpuid, &mask);

	if (pthread_setaffinity_np(pthread_self(), sizeof(mask), &mask) < 0)
		cerr << "set thread affinity failed" << endl;

	printf("Bind Track process to CPU %d\n", cpuid); 

	// 0.初始化若干个随机颜色
	const int CNUM = 20; //最多同时跟踪目标数 仅在显示时使用
	cv::RNG rng(0xFFFFFFFF); //RNG类是opencv里C++的随机数产生器
	cv::Scalar_<int> randColor[CNUM]; //CNUM
    //Scalar是一个包含四个元素vector的模板类，他继承于Vec，由于它继承于Vec<Tp, 4> ，被广泛用于传递像素值
	for (int i = 0; i < CNUM; i++)
		rng.fill(randColor[i], RNG::UNIFORM, 0, 256);

	// Config For Track
	double iouThreshold = 0.3;
	vector<KalmanTracker> trackers;
	KalmanTracker::kf_count = 0; // tracking id relies on this.在初始时置为0.
	
	vector<detection> frameTrackingResult;

	double first_time,last_time;
	int fps = 0;
	first_time=what_time_is_it_now();
    
    
    while (1) 
	{
		mtxQueueShow.lock();
		if (queueShow.empty()) {
			mtxQueueShow.unlock();
			usleep(1000);
		}
		// 如果下一个要显示的图片已经被处理好了 则进行追踪任务
		else if (idxShowImage == queueShow.top().frame_id) {
			cout << "待追踪的图片数: " << queueShow.size() << endl;
			cv::Mat img = queueShow.top().img; // 当前帧的图片
			vector<detection> detFrameData = queueShow.top().dets; // 当前帧的所有检测结果 
			if (trackers.size() == 0) // 追踪器是空(第一帧 或者 跟踪目标丢失)
			{
				//用第一帧的检测结果初始化跟踪器
				for (unsigned int i = 0; i < detFrameData.size(); i++)
				{
					KalmanTracker trk = KalmanTracker(detFrameData[i].bbox);
					trackers.push_back(trk);
				}
				// continue;
			}
			else
			{
				//3.1 预测已有跟踪器在当前帧的bb
				vector<cv::Rect_<float>> predictedBoxes;//预测bb
				predictedBoxes.clear();
				for (auto it = trackers.begin(); it != trackers.end();)
				{
					cv::Rect_<float> pBox = (*it).predict();
					if (pBox.x >= 0 && pBox.y >= 0)
					{
						predictedBoxes.push_back(pBox);
						it++;
					}
					else
					{
						it = trackers.erase(it);//bb不合理的tracker会被清除
						//cerr << "Box invalid at frame: " << idxShowImage << endl;
					}
				}
				//cout << "3.1 over" << endl;
				// 3.2. 使用匈牙利算法进行匹配
				vector<vector<double>> iouMatrix;
				iouMatrix.clear();
				unsigned int trkNum = 0;
				unsigned int detNum = 0;
				trkNum = predictedBoxes.size(); //由上一帧预测出来的结果
				detNum = detFrameData.size(); //当前帧的所有检测结果的 视作传感器的结果
				iouMatrix.resize(trkNum, vector<double>(detNum, 0)); //提前开好空间 避免频繁重定位
				for (unsigned int i = 0; i < trkNum; i++) // compute iou matrix as a distance matrix
				{
					for (unsigned int j = 0; j < detNum; j++)
					{
						// use 1-iou because the hungarian algorithm computes a minimum-cost assignment.
						iouMatrix[i][j] = 1 - box_iou(predictedBoxes[i], detFrameData[j].bbox);
					}
				}

				HungarianAlgorithm HungAlgo;
				vector<int> assignment; //匹配结果 给每一个trk找一个det
				assignment.clear();
                if(trkNum!=0)
                {
				    HungAlgo.Solve(iouMatrix, assignment);//匈牙利算法核心
                }
                    // find matches, unmatched_detections and unmatched_predictions
				set<int> unmatchedDetections; // 没有被配对的检测框 说明有新目标出现
				set<int> unmatchedTrajectories; // 没有被配对的追踪器 说明有目标消失
				set<int> allItems;
				set<int> matchedItems;
				vector<cv::Point> matchedPairs; // 最终配对结果 trk-det
				unmatchedTrajectories.clear();
				unmatchedDetections.clear();
				allItems.clear();
				matchedItems.clear();
				if (detNum > trkNum) // 检测框的数量 大于 现存追踪器的数量
				{
					for (unsigned int n = 0; n < detNum; n++)
						allItems.insert(n);
					for (unsigned int i = 0; i < trkNum; ++i)
						matchedItems.insert(assignment[i]);
					set_difference( allItems.begin(), allItems.end(),
									matchedItems.begin(), matchedItems.end(),
									insert_iterator<set<int>>(unmatchedDetections, unmatchedDetections.begin()));
						/*
							set_difference, 求集合1与集合2的差集 即可以找到没有被追踪的 det
							参数：第一个集合的开始位置，第一个集合的结束位置，
								第二个参数的开始位置，第二个参数的结束位置，
								结果集合的插入迭代器。
						*/
				}
				else if (detNum < trkNum) // 检测框的数量 小于 现存追踪器的数量; 追踪目标暂时消失
				{
					for (unsigned int i = 0; i < trkNum; ++i)
						if (assignment[i] == -1) // unassigned label will be set as -1 in the assignment algorithm
							unmatchedTrajectories.insert(i);
				}
				else //两者数量相等不做操作
					;

				// 过滤掉低IoU的匹配
				matchedPairs.clear();
				for (unsigned int i = 0; i < trkNum; ++i)
				{
					if (assignment[i] == -1) 
						continue;
					if (1 - iouMatrix[i][assignment[i]] < iouThreshold)
					{
						unmatchedTrajectories.insert(i);
						unmatchedDetections.insert(assignment[i]);
					}
					else
						matchedPairs.push_back(cv::Point(i, assignment[i])); // 符合条件 成功配对
				}
				//cout << "3.2 over" << endl;
				
				// 3.3. 更新跟踪器
				// update matched trackers with assigned detections.
				// each prediction is corresponding to a tracker
				int detIdx, trkIdx;
				for (unsigned int i = 0; i < matchedPairs.size(); i++)
				{
					trkIdx = matchedPairs[i].x;
					detIdx = matchedPairs[i].y;
					trackers[trkIdx].update(detFrameData[detIdx].bbox);
				}
				// 给未匹配到的检测框创建和初始化跟踪器
				// unmatchedTrajectories没有操作 所以有必要保存unmatchedTrajectories吗?(maybe not)
				for (auto umd : unmatchedDetections)
				{
					KalmanTracker tracker = KalmanTracker(detFrameData[umd].bbox);
					trackers.push_back(tracker);
				}
			}
			// 获得跟踪器输出
			int max_age = 1;
			int min_hits = 3;
			//m_time_since_update：tracker距离上次匹配成功间隔的帧数
			//m_hit_streak：tracker连续匹配成功的帧数
			frameTrackingResult.clear();
			for (auto it = trackers.begin(); it != trackers.end();)
			{
				// 输出条件：当前帧和前面2帧（连续3帧）匹配成功才记录
				if (((*it).m_time_since_update < 1) &&
					((*it).m_hit_streak >= min_hits || idxShowImage <= min_hits))//河狸
				{
					detection res;
					res.bbox = (*it).get_state();
					res.id = (*it).m_id + 1;
					frameTrackingResult.push_back(res);
					it++;
				}
				else
					it++;
				if (it != trackers.end() && (*it).m_time_since_update > max_age)//连续3帧还没匹配到，清除
					it = trackers.erase(it);
			}

            //cout << "3.3 over" << endl;
			// 绘图
			printf("%d\n", (int)frameTrackingResult.size());
			for (auto tb : frameTrackingResult)
			{
				cv::rectangle(img, tb.bbox, randColor[tb.id % CNUM], 2, 8, 0);
				cv::putText(img, "Id:"+to_string(tb.id), cv::Point(tb.bbox.x, tb.bbox.y), 1, 1, randColor[tb.id % CNUM], 1);
			}
			string a = to_string(fps) + "FPS";
			cv::putText(img, a, cv::Point(15, 15), 1, 1, cv::Scalar{0, 0, 255},2);
			
			// imshow("RK3399Pro",img);
			mtxQueueOutput.lock();
			queueOutput.push(img);
			mtxQueueOutput.unlock();

			idxShowImage++;
			queueShow.pop();
			mtxQueueShow.unlock();

			//计算FPS
			last_time=what_time_is_it_now();
			if(idxShowImage % 20 == 0)
			{
				//第一段时间有问题 会包含detection很长的一段初始化 pass
				if(idxShowImage != 20) Time_video += (last_time - first_time) / 1000.0;
				fps = (int)1000 / (last_time - first_time) * 20;
				first_time=what_time_is_it_now();
			}
		}
		else {
			//cout << "queueShow.top().frame_id: " << queueShow.top().frame_id << endl;
			mtxQueueShow.unlock();
		}
		// 因为此时一定允许过至少一次videoRead 因此frame_cnt一定不为0
		if (!bDetecting || waitKey(1) == 27) {
			printf("Track process is over.\n");
			break;
		}
	}
}

/*
	计算IoU
*/
double box_iou(cv::Rect_<float> bb_test, cv::Rect_<float> bb_gt)
{
	float in = (bb_test & bb_gt).area();
	float un = bb_test.area() + bb_gt.area() - in;

	if (un < DBL_EPSILON)
		return 0;

	return (double)(in / un);
}